% Demo showing how to use the simulated robot arm interactively to generate
% data.

mytwolink;  % define a 2D monkey arm with dynamixel motors!

t=0;
q=[0 0 ];
qd=[0 0 ];

sim_seconds = 200;
step_size_sec = .01;
save_every_nth_sample = 10;

nsamples = ceil(sim_seconds/step_size_sec/save_every_nth_sample)

samples= zeros(nsamples,3*length(q));

% [shoulder, elbow].
maxtau=[6.4 2.8];

tau = [0 0];

k=1;

% get the link angle limits from the model.
links = tl.links;
link1=links{1}.qlim;
link2=links{2}.qlim;
qmin = [link1(1) link2(1)]
qmax = [link1(2) link2(2)]

for i=1:sim_seconds/step_size_sec;

    
       
    
    [t2 q2 qd2 qdd] = myfdyn(tl, t,t+.01, tau,q,qd, qmin, qmax);
    t=t2(2);q=q2(2,:);qd=qd2(2,:);

    plot(tl,q);
    %title(num2str(t));
    
    % Collect data for training the model.
    % We need q, qd, and tau plus qtarget, qdtarget over time.
    
    
    % Let's store data every 100 msec, but integrate at 10 msec.
    
    if( mod(i,save_every_nth_sample) == 0 )
        samples(k,:) = [q qd tau];
        k=k+1;

        % generate training data
        % first stage of training uses only shoulder torques.
        % Keep the torque constant throughout the next sample period
        % so the mapping is accurate.
        tau =[maxtau(1)*abs(sin(.25*t)).*sin(10*t) 0];
        tau=max(-maxtau,min( maxtau, tau));
        t
    
    end
    
    
    
end
k

fid = fopen('armsampledata.dat','w');
fprintf(fid, '%f %f %f %f %f %f\n', samples');
fclose(fid);


% Now train a SOM on the data samples
P=samples(:,1:2:end)';
net = newsom(P,[10 10 5]);  % this is a 3D topography network.

%%
% We can visualize the network we have just created with PLOTSOM.
% 
% Each neuron is represented by a red dot at the location of its two weights.
% Initially all the neurons have the same weights in the middle of the vectors,
% so only one dot appears.

plotsom(net.iw{1,1},net.layers{1}.distances)

%%
% Now we train the map on the 1000 vectors for 1 epoch and replot the network
% weights.
% 
% After training, note that the layer of neurons has begun to self-organize so
% that each neuron now classifies a different region of the input space, and
% adjacent (connected) neurons respond to adjacent regions.

net.trainParam.epochs = 100;
net = train(net,P);

w=net.iw{1,1};
plotsom(net.iw{1,1},net.layers{1}.distances)

%%

% now train using STDP on forming a map from p(t),pd(t),tau(t) to p(t+100),pd(t+100)

% loop over the samples

% find the winners

% find the new winner for the next sample, ignoring tau.

% run STDP rule with the activity in these two map locations.

%% 
% Now test the network to see if stimulating a spot will cause the
% simulated arm to move to a specific spot.
% Why would this work?  
% 1. if proprioceptive target spot is interpreted by CB as a goal, then
% input from CB won't stop until goal achieved? (We don't have CB???)
% 2. If arm spends most time stopped at qd=0 and q=?, then organization
% should be along principal component, which explains most of data; that
% should be



